﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace g3
{
	// ported from WildMagic 5 
	// https://www.geometrictools.com/Downloads/Downloads.html

	public class DistRay3Ray3
	{
		Ray3d ray1;
		public Ray3d Ray1
		{
			get { return ray1; }
			set { ray1 = value; DistanceSquared = -1.0; }
		}

		Ray3d ray2;
		public Ray3d Ray2
		{
			get { return ray2; }
			set { ray2 = value; DistanceSquared = -1.0; }
		}

		public double DistanceSquared = -1.0;

		public Vector3d Ray1Closest;
		public double Ray1Parameter;
		public Vector3d Ray2Closest;
		public double Ray2Parameter;


		public DistRay3Ray3(Ray3d ray1, Ray3d ray2)
		{
			this.ray1 = ray1; this.ray2 = ray2;
		}

		static public double MinDistance(Ray3d r1, Ray3d r2)
		{
			return new DistRay3Ray3(r1, r2).Get();
		}
		static public double MinDistanceRay2Param(Ray3d r1, Ray3d r2)
		{
			return new DistRay3Ray3(r1, r2).Compute().Ray2Parameter;
		}


		public DistRay3Ray3 Compute()
		{
			GetSquared();
			return this;
		}

		public double Get()
		{
			return Math.Sqrt(GetSquared());
		}


		public double GetSquared()
		{
			if (DistanceSquared >= 0)
			{
				return DistanceSquared;
			}

			Vector3d diff = ray1.Origin - ray2.Origin;
			double a01 = -ray1.Direction.Dot(ray2.Direction);
			double b0 = diff.Dot(ray1.Direction);
			double c = diff.LengthSquared;
			double det = Math.Abs(1.0 - a01 * a01);
			double b1, s0, s1, sqrDist;

			if (det >= MathUtil.ZeroTolerance)
			{
				// Rays are not parallel.
				b1 = -diff.Dot(ray2.Direction);
				s0 = a01 * b1 - b0;
				s1 = a01 * b0 - b1;

				if (s0 >= 0)
				{
					if (s1 >= 0)
					{ // region 0 (interior)
					  // Minimum at two interior points of rays.
						double invDet = (1.0) / det;
						s0 *= invDet;
						s1 *= invDet;
						sqrDist = s0 * (s0 + a01 * s1 + (2.0) * b0) +
							s1 * (a01 * s0 + s1 + (2.0) * b1) + c;
					}
					else
					{ // region 3 (side)
						s1 = 0;
						if (b0 >= 0)
						{
							s0 = 0;
							sqrDist = c;
						}
						else
						{
							s0 = -b0;
							sqrDist = b0 * s0 + c;
						}
					}
				}
				else
				{
					if (s1 >= 0)
					{  // region 1 (side)
						s0 = 0;
						if (b1 >= 0)
						{
							s1 = 0;
							sqrDist = c;
						}
						else
						{
							s1 = -b1;
							sqrDist = b1 * s1 + c;
						}
					}
					else
					{  // region 2 (corner)
						if (b0 < 0)
						{
							s0 = -b0;
							s1 = 0;
							sqrDist = b0 * s0 + c;
						}
						else
						{
							s0 = 0;
							if (b1 >= 0)
							{
								s1 = 0;
								sqrDist = c;
							}
							else
							{
								s1 = -b1;
								sqrDist = b1 * s1 + c;
							}
						}
					}
				}
			}
			else
			{
				// Rays are parallel.
				if (a01 > 0)
				{
					// Opposite direction vectors.
					s1 = 0;
					if (b0 >= 0)
					{
						s0 = 0;
						sqrDist = c;
					}
					else
					{
						s0 = -b0;
						sqrDist = b0 * s0 + c;
					}
				}
				else
				{
					// Same direction vectors.
					if (b0 >= 0)
					{
						b1 = -diff.Dot(ray2.Direction);
						s0 = 0;
						s1 = -b1;
						sqrDist = b1 * s1 + c;
					}
					else
					{
						s0 = -b0;
						s1 = 0;
						sqrDist = b0 * s0 + c;
					}
				}
			}

			Ray1Closest = ray1.Origin + s0 * ray1.Direction;
			Ray2Closest = ray2.Origin + s1 * ray2.Direction;
			Ray1Parameter = s0;
			Ray2Parameter = s1;

			// Account for numerical round-off errors.
			if (sqrDist < 0)
			{
				sqrDist = 0;
			}
			DistanceSquared = sqrDist;

			return sqrDist;
		}


	}
}
